/************************************************************************
 * MechSys - Open Library for Mechanical Systems                        *
 * Copyright (C) 2013 Maziar Gholami Korzani and Sergio Galindo Torres  *
 *                                                                      *
 * This program is free software: you can redistribute it and/or modify *
 * it under the terms of the GNU General Public License as published by *
 * the Free Software Foundation, either version 3 of the License, or    *
 * any later version.                                                   *
 *                                                                      *
 * This program is distributed in the hope that it will be useful,      *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of       *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the         *
 * GNU General Public License for more details.                         *
 *                                                                      *
 * You should have received a copy of the GNU General Public License    *
 * along with this program. If not, see <http://www.gnu.org/licenses/>  *
 ************************************************************************/

#ifndef MECHSYS_SPH_DOMAIN_H
#define MECHSYS_SPH_DOMAIN_H

// Std Lib
#include <stdio.h>			/// for NULL
#include <algorithm>		/// for min,max
#include <iostream>
#include <cstring>
#include <cmath>
#include <omp.h>

#include <Particle.h>

// HDF File Output
#include <hdf5.h>
#include <hdf5_hl.h>

// MechSys
#include <mechsys/linalg/matvec.h>
#include <mechsys/util/stopwatch.h>
#include <mechsys/util/string.h>

namespace SPH {



class Domain
{
public:

    // Constructor
    Domain();
    // Destructor
    ~Domain ();

    // Methods
    void DelParticles			(int const & Tags);										///< Delete particles by tag

    void Output					(char const * TheFileKey, int DigitNo, int BFileNo, int FileNo);	///< Creating output function

    void CalcProp				(Particle * P1, Particle * P2);							///< Calculate properties for each lattice node
    double Kernel				(size_t Dim, size_t KT, double r, double h);
    void CellInitiate			();														///< Find the size of the domain as a cube, make cell and HOC
    void ListGenerate			();														///< Generate linked-list
    void CellReset				();														///< Reset HOC to initial value of -1
    void ForceCalc 				();														///< Calculate average velocity and force of a rigid body
    void CalcFinal				();														///< Averaging properties for each lattice node
    void InteractionsCalc		();														///< Reset the interaction array and make the first Interaction and PInteraction array
    void NeighbourSearch		(int q1);												///< Find neighbor cells and make interaction for cell(q1,q2,q3)

    void RigidBodyOut 			();														///< Output for rigid body
    void WriteXDMF				(char const * FileKey);									///< Save a XDMF file for visualization
    void Load					(char const * FileKey);									///< Load the domain from a file

    // Data
    Array <Particle*>			Particles;     	 	///< Array of particles

    size_t 						Dimension;    	  	///< Dimension of the problem

    Vec3_t                  	TRPR;				///< Top right-hand point at rear of the domain as a cube
    Vec3_t                  	BLPF;           	///< Bottom left-hand point At front side of the domain as a cube
    Vec3_t                  	CellSize;      		///< Calculated cell size according to (cell size >= 2h)
    int		                	CellNo[3];      	///< No. of cells for linked list
    int						*** HOC;				///< Array of (Head of Chain) for each cell
    double 						Cellfac;			///< Factor which should be multiplied by h to change the size of cells (Min 2)
	double 						hmax;				///< Max of h
	double						hfac;				///< Factor of h for averaging

	size_t						Nx;					///< No of lattice nodes in X direction
	size_t						Ny;					///< No of lattice nodes in Y direction
	size_t						Nz;					///< No of lattice nodes in Z direction

    Vec3_t						RBForce;			///< Rigid body force at each time step
    Vec3_t						RBForceVis;			///< Rigid body force at each time step
    Array <Vec3_t>				ForceA;				///< Array of forces for rigid body in each time step
    Array <Vec3_t>				ForceB;				///< Array of forces for rigid body in each time step
	Array <double>				AvgVel;     	 	///< Array of average velocity in each time step
    bool						RigidBody; 			///< If it is true, A rigid body with RTag will be considered

    size_t						Nproc;				///< No of threads which are going to use in parallel calculation
    size_t						KernelType;			///< Selecting variable to choose a kernel
    int							RBTag;
    omp_lock_t 					dom_lock;			///< Open MP lock to lock Interactions array
};

// Constructor
inline Domain::Domain ()
{
	Nproc			= 1;
	KernelType		= 0;

	Nx = Ny = Nz	= 0;

	Dimension		= 2;

	hfac			= 1.0;
	hmax			= 0.0;
	if (KernelType==4)Cellfac = 3.0; else Cellfac = 2.0;

	RigidBody		= false;
	RBForce			= 0.0;
	RBForceVis		= 0.0;
    omp_init_lock (&dom_lock);

}

inline Domain::~Domain ()
{
	size_t Max = Particles.Size();
	for (size_t i=1; i<=Max; i++)  Particles.DelItem(Max-i);
}

inline void Domain::CalcProp(Particle * P1, Particle * P2)
{
	double h;
	if (P1->ID!=100)  h=P1->h; else h=P2->h;
    double rij	= norm((P1->x - P2->x));
    double K	= Kernel(Dimension, KernelType, rij, h);

    if (K != 0.0)
    {
		if (P1->ID==100)
		{
		    omp_set_lock(&P1->my_lock);
//			P1->Density 	+= P2->Density;
//			P1->Pressure 	+= P2->Pressure;
//			P1->v 			+= P2->v;
//	    	P1->No++;
			P1->Density 	+= P2->Mass * K;
			P1->Pressure 	+= P2->Pressure * P2->Mass / P2->Density * K;
			P1->v 			+= P2->v * P2->Mass / P2->Density * K;
	    	P1->ZWab   		+= P2->Mass/P2->Density* K;
	        omp_unset_lock(&P1->my_lock);
		}
		else
		{
		    omp_set_lock(&P2->my_lock);
//			P2->Density 	+= P1->Density;
//			P2->Pressure 	+= P1->Pressure;
//			P2->v 			+= P1->v;
//	    	P2->No++;
			P2->Density 	+= P1->Mass * K;
			P2->Pressure 	+= P1->Pressure * P1->Mass / P1->Density * K;
			P2->v 			+= P1->v * P1->Mass / P1->Density * K;
	    	P2->ZWab   		+= P1->Mass/P1->Density* K;
	        omp_unset_lock(&P2->my_lock);
		}
    }
}

inline double Domain::Kernel(size_t Dim, size_t KT, double r, double h)
{
	double C;
	double q = r/h;

	switch (KT)
    {case 0:
    	// Qubic Spline
    	Dim ==2 ? C = 10.0/(7.0*h*h*M_PI) : C = 1.0/(h*h*h*M_PI);

		if ((q>=0.0)&&(q<1.0))	return C*(1.0-(3.0/2.0)*q*q+(3.0/4.0)*q*q*q);
		else if (q<2.0)			return C*((1.0/4.0)*(2.0-q)*(2.0-q)*(2.0-q));
		else					return 0.0;

		break;
    case 1:
    	// Quadratic
    	Dim ==2 ? C = 2.0/(h*h*M_PI) : C = 5.0/(4.0*h*h*h*M_PI);

    	if (q<2.0)				return C*(3.0/4.0-(3.0/4.0)*q+(3.0/16.0)*q*q);
		else					return 0.0;

    	break;
    case 2:
    	// Quintic
    	Dim ==2 ? C = 7.0/(4.0*h*h*M_PI) : C = 7.0/(8.0*h*h*h*M_PI);

    	if (q<2.0)				return C*pow((1.0-q/2.0),4.0)*(2.0*q+1.0);
		else					return 0.0;

    	break;
    case 3:
    	// Gaussian with compact support
    	Dim ==2 ? C = 1.0/(h*h*M_PI) : C = 1.0/(h*h*h*pow(M_PI,(3.0/2.0)));

    	if (q<=2.0)				return C*exp(-q*q);
		else					return 0.0;

    	break;
	case 4:
		// Quintic Spline
		Dim ==2 ? C = 7.0/(478.0*h*h*M_PI) : C = 1.0/(120.0*h*h*h*M_PI);

		if ((q>=0.0)&&(q<1.0))	return C*(pow((3.0-q),5.0)-6.0*pow((2.0-q),5.0)+15.0*pow((1.0-q),5.0));
		else if (q<2.0)			return C*(pow((3.0-q),5.0)-6.0*pow((2.0-q),5.0));
		else if (q<3.0)			return C*(pow((3.0-q),5.0));
		else					return 0.0;

		break;
   default:
	   	std::cout << "Kernel Type No is out of range. Please correct it and run again" << std::endl;
		std::cout << "0 => Qubic Spline" << std::endl;
		std::cout << "1 => Quadratic" << std::endl;
		std::cout << "2 => Quintic" << std::endl;
		std::cout << "3 => Gaussian with compact support of q<2" << std::endl;
		std::cout << "4 => Quintic Spline" << std::endl;
	    abort();
	    break;
    }
}

inline void Domain::CellInitiate ()
{
	// Calculate Domain Size
	BLPF = Particles[0]->x;
	TRPR = Particles[0]->x;
	hmax = Particles[0]->h;

	for (size_t i=0; i<Particles.Size(); i++)
    {
		if (Particles[i]->x(0) > TRPR(0)) TRPR(0) = Particles[i]->x(0);
        if (Particles[i]->x(1) > TRPR(1)) TRPR(1) = Particles[i]->x(1);
        if (Particles[i]->x(2) > TRPR(2)) TRPR(2) = Particles[i]->x(2);

        if (Particles[i]->x(0) < BLPF(0)) BLPF(0) = Particles[i]->x(0);
        if (Particles[i]->x(1) < BLPF(1)) BLPF(1) = Particles[i]->x(1);
        if (Particles[i]->x(2) < BLPF(2)) BLPF(2) = Particles[i]->x(2);

        if (Particles[i]->h > hmax) hmax=Particles[i]->h;
    }

	TRPR+=hmax;
	BLPF-=hmax;

	switch (Dimension)
	{case 2:
		if (double (ceil(((TRPR(0)-BLPF(0))/(Cellfac*hmax)))-((TRPR(0)-BLPF(0))/(Cellfac*hmax)))<(hmax/10.0))
			CellNo[0] = int(ceil((TRPR(0)-BLPF(0))/(Cellfac*hmax)));
		else
			CellNo[0] = int(floor((TRPR(0)-BLPF(0))/(Cellfac*hmax)));

		if (double (ceil(((TRPR(1)-BLPF(1))/(Cellfac*hmax)))-((TRPR(1)-BLPF(1))/(Cellfac*hmax)))<(hmax/10.0))
			CellNo[1] = int(ceil((TRPR(1)-BLPF(1))/(Cellfac*hmax)));
		else
			CellNo[1] = int(floor((TRPR(1)-BLPF(1))/(Cellfac*hmax)));

		CellNo[2] = 1;

		CellSize  = Vec3_t ((TRPR(0)-BLPF(0))/CellNo[0],(TRPR(1)-BLPF(1))/CellNo[1],0.0);
		break;

	case 3:
		if (double (ceil(((TRPR(0)-BLPF(0))/(Cellfac*hmax)))-((TRPR(0)-BLPF(0))/(Cellfac*hmax)))<(hmax/10.0))
			CellNo[0] = int(ceil((TRPR(0)-BLPF(0))/(Cellfac*hmax)));
		else
			CellNo[0] = int(floor((TRPR(0)-BLPF(0))/(Cellfac*hmax)));

		if (double (ceil(((TRPR(1)-BLPF(1))/(Cellfac*hmax)))-((TRPR(1)-BLPF(1))/(Cellfac*hmax)))<(hmax/10.0))
			CellNo[1] = int(ceil((TRPR(1)-BLPF(1))/(Cellfac*hmax)));
		else
			CellNo[1] = int(floor((TRPR(1)-BLPF(1))/(Cellfac*hmax)));

		if (double (ceil(((TRPR(2)-BLPF(2))/(Cellfac*hmax)))-((TRPR(2)-BLPF(2))/(Cellfac*hmax)))<(hmax/10.0))
			CellNo[2] = int(ceil((TRPR(2)-BLPF(2))/(Cellfac*hmax)));
		else
			CellNo[2] = int(floor((TRPR(2)-BLPF(2))/(Cellfac*hmax)));

		CellSize  = Vec3_t ((TRPR(0)-BLPF(0))/CellNo[0],(TRPR(1)-BLPF(1))/CellNo[1],(TRPR(2)-BLPF(2))/CellNo[2]);
		break;

	default:
    	std::cout << "Please correct the dimension (2=>2D or 3=>3D) and run again" << std::endl;
		abort();
		break;
	}

    std::cout << "No of Cells in X Direction = " << CellNo[0] << std::endl;
    std::cout << "No of Cells in Y Direction = " << CellNo[1] << std::endl;
    std::cout << "No of Cells in Z Direction = " << CellNo[2] << std::endl;

    // Initiate Head of Chain array for Linked-List
    HOC = new int**[(int) CellNo[0]];
    for(int i =0; i<CellNo[0]; i++){
       HOC[i] = new int*[CellNo[1]];
       for(int j =0; j<CellNo[1]; j++){
           HOC[i][j] = new int[CellNo[2]];
           for(int k = 0; k<CellNo[2];k++){
              HOC[i][j][k] = -1;
           }
       }
    }
}

inline void Domain::ListGenerate ()
{
	int i, j, k, temp=0;
	switch (Dimension)
	{case 2:
		for (size_t a=0; a<Particles.Size(); a++)
		{
			i= (int) (floor((Particles[a]->x(0) - BLPF(0)) / CellSize(0)));
			j= (int) (floor((Particles[a]->x(1) - BLPF(1)) / CellSize(1)));

			if (i<0)
            {
                    if ((BLPF(0) - Particles[a]->x(0)) <= hmax) i=0;
                            else std::cout<<"Leaving i<0"<<std::endl;
            }
            if (j<0)
            {
                    if ((BLPF(1) - Particles[a]->x(1)) <= hmax) j=0;
                            else std::cout<<"Leaving j<0"<<std::endl;
            }
			if (i>=CellNo[0])
			{
					if ((Particles[a]->x(0) - TRPR(0)) <= hmax) i=CellNo[0]-1;
							else std::cout<<"Leaving i>=CellNo"<<std::endl;
			}
            if (j>=CellNo[1])
            {
                    if ((Particles[a]->x(1) - TRPR(1)) <= hmax) j=CellNo[1]-1;
                            else std::cout<<"Leaving j>=CellNo"<<std::endl;
            }

			temp = HOC[i][j][0];
			HOC[i][j][0] = a;
			Particles[a]->LL = temp;
		}
		break;

	case 3:
		for (size_t a=0; a<Particles.Size(); a++)
		{
			i= (int) (floor((Particles[a]->x(0) - BLPF(0)) / CellSize(0)));
			j= (int) (floor((Particles[a]->x(1) - BLPF(1)) / CellSize(1)));
			k= (int) (floor((Particles[a]->x(2) - BLPF(2)) / CellSize(2)));

            if (i<0)
            {
                    if ((BLPF(0) - Particles[a]->x(0))<=hmax) i=0;
                            else std::cout<<"Leaving"<<std::endl;
            }
            if (j<0)
            {
                    if ((BLPF(1) - Particles[a]->x(1))<=hmax) j=0;
                            else std::cout<<"Leaving"<<std::endl;
            }
            if (k<0)
            {
                    if ((BLPF(2) - Particles[a]->x(2))<=hmax) k=0;
                            else std::cout<<"Leaving"<<std::endl;
            }
			if (i>=CellNo[0])
			{
					if ((Particles[a]->x(0) - TRPR(0))<=hmax) i=CellNo[0]-1;
							else std::cout<<"Leaving"<<std::endl;
			}
            if (j>=CellNo[1])
            {
                    if ((Particles[a]->x(1) - TRPR(1))<=hmax) j=CellNo[1]-1;
                            else std::cout<<"Leaving"<<std::endl;
            }
            if (k>=CellNo[2])
            {
                    if ((Particles[a]->x(2) - TRPR(2))<=hmax) k=CellNo[2]-1;
                            else std::cout<<"Leaving"<<std::endl;
            }

            temp = HOC[i][j][k];
			HOC[i][j][k] = a;
			Particles[a]->LL = temp;
		}
		break;

	default:
    	std::cout << "Please correct the dimension (2=>2D or 3=>3D) and run again" << std::endl;
		abort();
		break;
	}
}

inline void Domain::InteractionsCalc()
{
	#pragma omp parallel for schedule (dynamic) num_threads(Nproc)
	for (int q1=0; q1<CellNo[0]; q1++)
	{
		NeighbourSearch(q1);
	}
}

inline void Domain::NeighbourSearch(int q1)
{
	int q3,q2;
	Array<std::pair<size_t,size_t> > LocalPairs;

	for (q3=0;q3<CellNo[2]; q3++)
	for (q2=0;q2<CellNo[1]; q2++)
	{
		if (HOC[q1][q2][q3]==-1) continue;
		else
		{
			int temp1, temp2;
			temp1 = HOC[q1][q2][q3];

			while (temp1 != -1)
			{
				// The current cell  => self cell interactions
				temp2 = Particles[temp1]->LL;
				while (temp2 != -1)
				{
					LocalPairs.Push(std::make_pair(temp1, temp2));
					temp2 = Particles[temp2]->LL;
				}

				// (q1 + 1, q2 , q3)
				if (q1+1< CellNo[0])
				{
					temp2 = HOC[q1+1][q2][q3];
					while (temp2 != -1)
					{
						LocalPairs.Push(std::make_pair(temp1, temp2));
						temp2 = Particles[temp2]->LL;
					}
				}

				// (q1 + a, q2 + 1, q3) & a[-1,1]
				if (q2+1< CellNo[1])
				{
					for (int i = q1-1; i <= q1+1; i++)
					{
						if (i<CellNo[0] && i>=0)
						{
							temp2 = HOC[i][q2+1][q3];
							while (temp2 != -1)
							{
								LocalPairs.Push(std::make_pair(temp1, temp2));
								temp2 = Particles[temp2]->LL;
							}
						}
					}
				}

				// (q1 + a, q2 + b, q3 + 1) & a,b[-1,1] => all 9 cells above the current cell
				if (q3+1< CellNo[2])
				{
					for (int j=q2-1; j<=q2+1; j++)
					for (int i=q1-1; i<=q1+1; i++)
					{
						if (i<CellNo[0] && i>=0 && j<CellNo[1] && j>=0)
						{
							temp2 = HOC[i][j][q3+1];
							while (temp2 != -1)
							{
								LocalPairs.Push(std::make_pair(temp1, temp2));
								temp2 = Particles[temp2]->LL;
							}
						}
					}
				}
				temp1 = Particles[temp1]->LL;
			}
		}
	}

	for (size_t i=0; i<LocalPairs.Size();i++)
		if (Particles[LocalPairs[i].first]->IsFree || Particles[LocalPairs[i].second]->IsFree)
			if (Particles[LocalPairs[i].first]->ID==100 || Particles[LocalPairs[i].second]->ID==100)
				CalcProp(Particles[LocalPairs[i].first],Particles[LocalPairs[i].second]);

	LocalPairs.Clear();
}

inline void Domain::CalcFinal()
{
	#pragma omp parallel for schedule (dynamic) num_threads(Nproc)
    for (size_t a=0; a < Particles.Size(); a++)
    {
   		if (Particles[a]->ID == 100)
   		{
//			Particles[a]->Density = Particles[a]->Density / Particles[a]->No;
//			Particles[a]->Pressure = Particles[a]->Pressure / Particles[a]->No;
//			Particles[a]->v = Particles[a]->v / Particles[a]->No;
			Particles[a]->Density = Particles[a]->Density / Particles[a]->ZWab;
			Particles[a]->Pressure = Particles[a]->Pressure / Particles[a]->ZWab;
			Particles[a]->v = Particles[a]->v / Particles[a]->ZWab;
		}
    }
}

inline void Domain::ForceCalc ()
{
	double AvgVelocity = 0.0;
	int j = 0;

	#pragma omp parallel for  schedule (static) num_threads(Nproc)
	for (size_t i=0; i<Particles.Size()-Nx*Ny*Nz; i++)
	{
		if (Particles[i]->ID==RBTag)
		{
			omp_set_lock(&dom_lock);
			AvgVelocity = Particles[i]->v(0);
			omp_unset_lock(&dom_lock);
		}
	}

	if (AvgVelocity == 0.0)
	{
		int temp;
		for (int q3=0; q3<CellNo[2]; q3++)
		for (int q2=0; q2<CellNo[1]; q2++)
		for (int q1=CellNo[0]-4; q1<CellNo[0]; q1++)
		{
			if (HOC[q1][q2][q3]==-1) continue;
			else
			{
				temp = HOC[q1][q2][q3];
				while (temp != -1)
				{
					if (Particles[temp]->IsFree)
					{
					AvgVelocity += Particles[temp]->v(0);
					j++;
					}
					temp = Particles[temp]->LL;
				}
			}
		}
		AvgVelocity = AvgVelocity / j;
	}

	AvgVel.Push(AvgVelocity);
	ForceA.Push(RBForce);
	ForceB.Push(RBForceVis);
}

inline void Domain::CellReset ()
{
	for(int i =0; i<CellNo[0]; i++)
	for(int j =0; j<CellNo[1]; j++)
	for(int k =0; k<CellNo[2]; k++)
	{
	  HOC[i][j][k] = -1;
	}
}

inline void Domain::Output (char const * TheFileKey, int DigitNo, int BFileNo, int FileNo)
{
    if (Dimension<=1 || Dimension>3)
    {
    	std::cout << "Please correct the dimension (2=>2D or 3=>3D) and run again" << std::endl;
		abort();
    }
    String fn;
    double x,y,z;

    for (int a=BFileNo; a <= FileNo; a++)
    {
        fn.Printf("%s_%04d", TheFileKey, a);
        Load(fn.CStr());

        std::cout << "\n" << "Output " << fn.CStr() << " has been loaded" << std::endl;

        if (a==BFileNo) CellInitiate();

		size_t b = Particles.Size()-Nx*Ny*Nz;
		for (size_t k=0; k<Nz; k++)
		for (size_t j=0; j<Ny; j++)
		for (size_t i=0; i<Nx; i++)
		{
			x = BLPF(0) + i*(TRPR(0)-BLPF(0))/Nx + 0.5*(TRPR(0)-BLPF(0))/Nx;
			if (Dimension==2 || Dimension==3) y = BLPF(1) + j*(TRPR(1)-BLPF(1))/Ny + 0.5*(TRPR(1)-BLPF(1))/Ny;
			else y=0.0;
			if (Dimension==3) z = BLPF(2) + k*(TRPR(2)-BLPF(2))/Nz + 0.5*(TRPR(2)-BLPF(2))/Nz;
			else z=0.0;

			Particles[b]->x			= Vec3_t(x,y,z);
	    	Particles[b]->v			= Vec3_t(0.0,0.0,0.0);
	    	Particles[b]->Pressure	= 0.0;
	    	Particles[b]->Density	= 0.0;
	    	Particles[b]->Mass		= 0.0;
	    	Particles[b]->h			= 0.0;
	    	Particles[b]->ID		= 100;
	    	Particles[b]->IsFree	= false;
			Particles[b]->No		= 1;
			Particles[b]->LL		= -1;
			Particles[b]->ZWab		= 0.0;
			b++;
		}

		ListGenerate();
        InteractionsCalc();
        CalcFinal();
        if (RigidBody) ForceCalc();
		WriteXDMF(fn.CStr());
		CellReset();
    }

    std::cout << "Finish"<< std::endl;

    if (RigidBody)
    {
    	RigidBodyOut();
        std::cout << "Rigid Body output has been generated"<< std::endl;
    }
}

inline void Domain::RigidBodyOut ()
{
	String fn("RigidBody");
    fn.append(".hdf5");

    hid_t file_id;
    file_id = H5Fcreate(fn.CStr(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

    float * Force       = new float[3*(ForceA.Size())];
    float * ForceVis    = new float[3*(ForceB.Size())];
    float * AvgVelocity = new float[  (AvgVel.Size())];

    for (size_t i=0;i<ForceA.Size();i++)
    {
			Force   [3*i  ] = float(ForceA[i](0));
			Force   [3*i+1] = float(ForceA[i](1));
			Force   [3*i+2] = float(ForceA[i](2));
			ForceVis[3*i  ] = float(ForceB[i](0));
			ForceVis[3*i+1] = float(ForceB[i](1));
			ForceVis[3*i+2] = float(ForceB[i](2));
			AvgVelocity[i ] = float(AvgVel[i]);
    }

    hsize_t dims[1];
    dims[0] = 3*ForceA.Size();
    String dsname;
    dsname.Printf("Force_Total");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,Force);
    dsname.Printf("Force_Viscosity");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,ForceVis);
    dims[0] = ForceA.Size();
    dsname.Printf("Average_Velocity");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,AvgVelocity);


    delete [] Force;
    delete [] ForceVis;
    delete [] AvgVelocity;

    //Closing the file
    H5Fflush(file_id,H5F_SCOPE_GLOBAL);
    H5Fclose(file_id);
}

inline void Domain::WriteXDMF (char const * FileKey)
{
	String fn(FileKey);
    fn.append(".hdf5");

    hid_t file_id;
    file_id = H5Fcreate(fn.CStr(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);

    float * Posvec   = new float[3*(Particles.Size()-Nx*Ny*Nz)];
    float * Velvec   = new float[3*(Particles.Size()-Nx*Ny*Nz)];
    float * Pressure = new float[  (Particles.Size()-Nx*Ny*Nz)];
    float * Density  = new float[  (Particles.Size()-Nx*Ny*Nz)];
    int   * Tag      = new int  [  (Particles.Size()-Nx*Ny*Nz)];

    for (size_t i=0;i<Particles.Size()-Nx*Ny*Nz;i++)
    {
			Posvec  [3*i  ] = float(Particles[i]->x(0));
			Posvec  [3*i+1] = float(Particles[i]->x(1));
			Posvec  [3*i+2] = float(Particles[i]->x(2));
			Velvec  [3*i  ] = float(Particles[i]->v(0));
			Velvec  [3*i+1] = float(Particles[i]->v(1));
			Velvec  [3*i+2] = float(Particles[i]->v(2));
			Pressure[i    ] = float(Particles[i]->Pressure);
			Density [i    ] = float(Particles[i]->Density);
			Tag     [i    ] = int  (Particles[i]->ID);
    }

    hsize_t dims[1];
    dims[0] = 3*(Particles.Size()-Nx*Ny*Nz);
    String dsname;
    dsname.Printf("Position");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,Posvec);
    dsname.Printf("Velocity");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,Velvec);
    dims[0] = (Particles.Size()-Nx*Ny*Nz);
    dsname.Printf("Pressure");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,Pressure);
    dsname.Printf("Density");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,Density);
    dsname.Printf("Tag");
    H5LTmake_dataset_int(file_id,dsname.CStr(),1,dims,Tag);


    delete [] Posvec;
    delete [] Velvec;
    delete [] Pressure;
    delete [] Density;
    delete [] Tag;

    float * VelvecG   = new float[3*Nx*Ny*Nz];
    float * PressureG = new float[  Nx*Ny*Nz];
    float * DensityG  = new float[  Nx*Ny*Nz];

    size_t c=0;
    for (size_t i=(Particles.Size()-Nx*Ny*Nz);i<Particles.Size();i++)
    {
			VelvecG  [3*c  ] = float(Particles[i]->v(0));
			VelvecG  [3*c+1] = float(Particles[i]->v(1));
			VelvecG  [3*c+2] = float(Particles[i]->v(2));
			PressureG[c    ] = float(Particles[i]->Pressure);
			DensityG [c    ] = float(Particles[i]->Density);
			c++;
     }

    dims[0] = 3*Nx*Ny*Nz;
    dsname.Printf("Velocity_Grid");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,VelvecG);
    dims[0] = Nx*Ny*Nz;
    dsname.Printf("Pressure_Grid");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,PressureG);
    dsname.Printf("Density_Grid");
    H5LTmake_dataset_float(file_id,dsname.CStr(),1,dims,DensityG);

    int N[1];
    dims[0] = 1;
    N[0] = Nx;
    dsname.Printf("Nx");
    H5LTmake_dataset_int(file_id,dsname.CStr(),1,dims,N);
    dims[0] = 1;
    N[0] = Ny;
    dsname.Printf("Ny");
    H5LTmake_dataset_int(file_id,dsname.CStr(),1,dims,N);
    dims[0] = 1;
    N[0] = Nz;
    dsname.Printf("Nz");
    H5LTmake_dataset_int(file_id,dsname.CStr(),1,dims,N);

    delete [] VelvecG;
    delete [] PressureG;
    delete [] DensityG;

    //Closing the file
    H5Fflush(file_id,H5F_SCOPE_GLOBAL);
    H5Fclose(file_id);

    //Writing xmf file
    std::ostringstream oss;

    //Grid base
    if (Dimension==2)
    {
        oss << "<?xml version=\"1.0\" ?>\n";
        oss << "<!DOCTYPE Xdmf SYSTEM \"Xdmf.dtd\" []>\n";
        oss << "<Xdmf Version=\"2.0\">\n";
        oss << " <Domain>\n";
        oss << "   <Grid Name=\"SPH Mesh\" GridType=\"Uniform\">\n";
        oss << "     <Topology TopologyType=\"2DCoRectMesh\" Dimensions=\"" << Ny << " " << Nx << "\"/>\n";
        oss << "     <Geometry GeometryType=\"ORIGIN_DXDY\">\n";
        oss << "       <DataItem Format=\"XML\" NumberType=\"Float\" Dimensions=\"2\"> 0.0 0.0\n";
        oss << "       </DataItem>\n";
        oss << "       <DataItem Format=\"XML\" NumberType=\"Float\" Dimensions=\"2\"> 1.0 1.0\n";
        oss << "       </DataItem>\n";
        oss << "     </Geometry>\n";
        oss << "     <Attribute Name=\"Density_Grid\" AttributeType=\"Scalar\" Center=\"Node\">\n";
        oss << "       <DataItem Dimensions=\"" << Ny << " " << Nx << "\" NumberType=\"Float\" Precision=\"4\" Format=\"HDF\">\n";
        oss << "        " << fn.CStr() <<":/Density_Grid\n";
        oss << "       </DataItem>\n";
        oss << "     </Attribute>\n";
        oss << "     <Attribute Name=\"Velocity_Grid\" AttributeType=\"Vector\" Center=\"Node\">\n";
        oss << "       <DataItem Dimensions=\"" << Ny << " " << Nx << " 3\" NumberType=\"Float\" Precision=\"4\" Format=\"HDF\">\n";
        oss << "        " << fn.CStr() <<":/Velocity_Grid\n";
        oss << "       </DataItem>\n";
        oss << "     </Attribute>\n";
        oss << "     <Attribute Name=\"Pressure_Grid\" AttributeType=\"Scalar\" Center=\"Node\">\n";
        oss << "       <DataItem Dimensions=\"" << Ny << " " << Nx << "\" NumberType=\"Float\" Precision=\"4\" Format=\"HDF\">\n";
        oss << "        " << fn.CStr() <<":/Pressure_Grid\n";
        oss << "       </DataItem>\n";
        oss << "     </Attribute>\n";
        oss << "   </Grid>\n";
    }
    else
    {
        oss << "<?xml version=\"1.0\" ?>\n";
        oss << "<!DOCTYPE Xdmf SYSTEM \"Xdmf.dtd\" []>\n";
        oss << "<Xdmf Version=\"2.0\">\n";
        oss << " <Domain>\n";
        oss << "   <Grid Name=\"SPH Mesh\" GridType=\"Uniform\">\n";
        oss << "     <Topology TopologyType=\"3DCoRectMesh\" Dimensions=\"" << Nz << " " << Ny << " " << Nx << "\"/>\n";
        oss << "     <Geometry GeometryType=\"ORIGIN_DXDYDZ\">\n";
        oss << "       <DataItem Format=\"XML\" NumberType=\"Float\" Dimensions=\"3\"> 0.0 0.0 0.0\n";
        oss << "       </DataItem>\n";
        oss << "       <DataItem Format=\"XML\" NumberType=\"Float\" Dimensions=\"3\"> 1.0 1.0 1.0\n";
        oss << "       </DataItem>\n";
        oss << "     </Geometry>\n";
        oss << "     <Attribute Name=\"Density_Grid\" AttributeType=\"Scalar\" Center=\"Node\">\n";
        oss << "       <DataItem Dimensions=\"" << Nz << " " << Ny << " " << Nx << "\" NumberType=\"Float\" Precision=\"4\" Format=\"HDF\">\n";
        oss << "        " << fn.CStr() <<":/Density_Grid\n";
        oss << "       </DataItem>\n";
        oss << "     </Attribute>\n";
        oss << "     <Attribute Name=\"Velocity_Grid\" AttributeType=\"Vector\" Center=\"Node\">\n";
        oss << "       <DataItem Dimensions=\"" << Nz << " " << Ny << " " << Nx << " 3\" NumberType=\"Float\" Precision=\"4\" Format=\"HDF\">\n";
        oss << "        " << fn.CStr() <<":/Velocity_Grid\n";
        oss << "       </DataItem>\n";
        oss << "     </Attribute>\n";
        oss << "     <Attribute Name=\"Pressure_Grid\" AttributeType=\"Scalar\" Center=\"Node\">\n";
        oss << "       <DataItem Dimensions=\"" << Nz << " " << Ny << " " << Nx << "\" NumberType=\"Float\" Precision=\"4\" Format=\"HDF\">\n";
        oss << "        " << fn.CStr() <<":/Pressure_Grid\n";
        oss << "       </DataItem>\n";
        oss << "     </Attribute>\n";
        oss << "   </Grid>\n";
    }

    //Particle base
    oss << "   <Grid Name=\"SPH Particles\" GridType=\"Uniform\">\n";
    oss << "     <Topology TopologyType=\"Polyvertex\" NumberOfElements=\"" << (Particles.Size()-Nx*Ny*Nz) << "\"/>\n";
    oss << "     <Geometry GeometryType=\"XYZ\">\n";
    oss << "       <DataItem Format=\"HDF\" NumberType=\"Float\" Precision=\"10\" Dimensions=\"" << (Particles.Size()-Nx*Ny*Nz) << " 3\" >\n";
    oss << "        " << fn.CStr() <<":/Position \n";
    oss << "       </DataItem>\n";
    oss << "     </Geometry>\n";
    oss << "     <Attribute Name=\"Velocity\" AttributeType=\"Vector\" Center=\"Node\">\n";
    oss << "       <DataItem Dimensions=\"" << (Particles.Size()-Nx*Ny*Nz) << " 3\" NumberType=\"Float\" Precision=\"10\" Format=\"HDF\">\n";
    oss << "        " << fn.CStr() <<":/Velocity \n";
    oss << "       </DataItem>\n";
    oss << "     </Attribute>\n";
    oss << "     <Attribute Name=\"Pressure\" AttributeType=\"Scalar\" Center=\"Node\">\n";
    oss << "       <DataItem Dimensions=\"" << (Particles.Size()-Nx*Ny*Nz) << "\" NumberType=\"Float\" Precision=\"10\"  Format=\"HDF\">\n";
    oss << "        " << fn.CStr() <<":/Pressure \n";
    oss << "       </DataItem>\n";
    oss << "     </Attribute>\n";
    oss << "     <Attribute Name=\"Density\" AttributeType=\"Scalar\" Center=\"Node\">\n";
    oss << "       <DataItem Dimensions=\"" << (Particles.Size()-Nx*Ny*Nz) << "\" NumberType=\"Float\" Precision=\"10\"  Format=\"HDF\">\n";
    oss << "        " << fn.CStr() <<":/Density \n";
    oss << "       </DataItem>\n";
    oss << "     </Attribute>\n";
    oss << "     <Attribute Name=\"Tag\" AttributeType=\"Scalar\" Center=\"Node\">\n";
    oss << "       <DataItem Dimensions=\"" << (Particles.Size()-Nx*Ny*Nz) << "\" NumberType=\"Int\" Format=\"HDF\">\n";
    oss << "        " << fn.CStr() <<":/Tag \n";
    oss << "       </DataItem>\n";
    oss << "     </Attribute>\n";
    oss << "   </Grid>\n";
    oss << " </Domain>\n";
    oss << "</Xdmf>\n";


    fn = FileKey;
    fn.append(".xmf");
    std::ofstream of(fn.CStr(), std::ios::out);
    of << oss.str();
    of.close();
}

inline void Domain::Load (char const * FileKey)
{

    String fn(FileKey);
    fn.append(".hdf5");

    if (!Util::FileExists(fn))
    {
        if (RigidBody)
        {
        	RigidBodyOut();
            std::cout << "Rigid Body output has been generated"<< std::endl;
        }
        throw new Fatal("File <%s> not found",fn.CStr());
    }

    hid_t file_id;
    file_id = H5Fopen(fn.CStr(), H5F_ACC_RDONLY, H5P_DEFAULT);

    int data[1];
    H5LTread_dataset_int(file_id,"/NP",data);

    float * Posvec   = new float[3*data[0]];
    float * Velvec   = new float[3*data[0]];
    float * Pressure = new float[  data[0]];
    float * Density  = new float[  data[0]];
    float * h		 = new float[  data[0]];
    float * Mass	 = new float[  data[0]];
    int   * Tag      = new int  [  data[0]];
    int   * IsFree   = new int  [  data[0]];
    float * Force    = new float[6];

    H5LTread_dataset_float(file_id,"Position",Posvec);
    H5LTread_dataset_float(file_id,"Velocity",Velvec);
    H5LTread_dataset_float(file_id,"Pressure",Pressure);
    H5LTread_dataset_float(file_id,"Density",Density);
    H5LTread_dataset_float(file_id,"Mass",Mass);
    H5LTread_dataset_float(file_id,"h",h);
    H5LTread_dataset_int(file_id,"Tag",Tag);
    H5LTread_dataset_int(file_id,"IsFree",IsFree);

    if (RigidBody)
    {
    	H5LTread_dataset_float(file_id,"Rigid_Body_Force",Force);
		RBForce(0) = Force[0];
		RBForce(1) = Force[1];
		RBForce(2) = Force[2];
		RBForceVis(0) = Force[3];
		RBForceVis(1) = Force[4];
		RBForceVis(2) = Force[5];
    }

//    std::cout<<"Real No Particles= "<<data[0]<<std::endl;
//    std::cout<<"Nx*Ny*Nz= "<<Nx*Ny*Nz<<std::endl;
//    std::cout<<"before loading= "<<Particles.Size()<<std::endl;

    size_t temp;
    if (Particles.Size()<(data[0]+Nx*Ny*Nz))
    {
    	temp = Particles.Size();
    	for (size_t i=temp; i<(data[0]+Nx*Ny*Nz); i++)
    		{
    		Particles.Push(new Particle(0,Vec3_t(Posvec[0],Posvec[1],Posvec[2]),Vec3_t(0.0,0.0,0.0),0.0,0.0,0.0,true));
    		}
    }

    if (Particles.Size()>(data[0]+Nx*Ny*Nz))
    {
    	temp = Particles.Size();
		for (size_t i=1; i<=(temp-(data[0]+Nx*Ny*Nz)); i++)  Particles.DelItem(temp-i);
    }

//    std::cout<<"After loading= "<<Particles.Size()<<std::endl;

	#pragma omp parallel for schedule (static) num_threads(Nproc)
    for (int i=0; i<data[0]; i++)
    {
    	Particles[i]->x			= Vec3_t(Posvec[3*i],Posvec[3*i+1],Posvec[3*i+2]);
    	Particles[i]->v			= Vec3_t(Velvec[3*i],Velvec[3*i+1],Velvec[3*i+2]);
    	Particles[i]->Pressure	= Pressure[i];
    	Particles[i]->Density	= Density[i];
    	Particles[i]->Mass		= Mass[i];
    	Particles[i]->h			= hfac*h[i];
    	Particles[i]->ID		= Tag[i];
		Particles[i]->No		= 1;
		Particles[i]->LL		= -1;
		Particles[i]->ZWab		= 0.0;

    	if (IsFree[i]==0) Particles[i]->IsFree = false;
    	else Particles[i]->IsFree = true;
    }

    delete [] Posvec;
    delete [] Velvec;
    delete [] Pressure;
    delete [] Density;
    delete [] h;
    delete [] Mass;
    delete [] Tag;
    delete [] IsFree;
    delete [] Force;

    H5Fflush(file_id,H5F_SCOPE_GLOBAL);
    H5Fclose(file_id);
}

}; // namespace SPH

#endif // MECHSYS_SPH_DOMAIN_H
